#ifndef __LAYERED_COSTMAP_H_
#define __LAYERED_COSTMAP_H_

#include "layer.h"
#include "costmap_2d.h"
using namespace std;

namespace costmap_2d{

    class Layer;    //前向引用

    class LayeredCostmap{
        public:
            LayeredCostmap(std::string global_frame, std::string base_frame, bool rolling_window);
            ~LayeredCostmap();

            void touch(double x, double y, double* min_x, double* min_y, double* max_x, double* max_y);
            void updateWithMax(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
            
            void addPlugin(boost::shared_ptr<Layer> plugin);
            std::vector<boost::shared_ptr<Layer> >* getPlugins();

            void resizeMap(unsigned int width, unsigned int height, double resolution, double origin_x,
                double origin_y);
            void updateMap(double robot_x, double robot_y, double robot_yaw);

            Costmap2D* getCostmap2D();           
            bool isRolling();
            string getGlobalFrameID();
            string getBaseFrameID();

        protected:
            Costmap2D costmap_2d_;
            bool rolling_window_;
            std::string global_frame_;
            std::string robot_base_frame_;
            std::vector<boost::shared_ptr<Layer>> plugins_;
            double minx_, miny_, maxx_, maxy_;  //代价地图边界坐标
    };

}   // namespace costmap_2d

#endif